#include "Camera.hpp"
#include "Controller.hpp"
#include "ControlPoint.hpp"
#include "gl.hpp"
#include "GlWindow.hpp"
#include "Intersector.hpp"
#include "MeshAlgorithm.hpp"
#include "Plane.hpp"
#include "ViewPoint.hpp"
#include "ViewScene.hpp"
#include <Fl/Fl.H>
#include <limits>
#include <math.h>
#include "MeshView.hpp"
#include "UiUtil.hpp"
#include <boost/bind.hpp>

ControlPoint::ControlPoint(ViewPoint* aViewPoint,
                           Controller* aController)
{
   ViewScene::SelectedVertsChanged.connect( boost::bind(&ControlPoint::HandleSelectionChange, this) );
   mViewPoint = aViewPoint;
   mController = aController;
   mScaleMode = mRotationMode = 0;
   mFreeTranslateMode = mTranslateMode = false;
   mAxisDown = -1;
   mIsVisible = false;
}
//namespace
//{
//   void Unproject(Vec2 pos, Vec3& orig, Vec3& dir)
//   {
//      GlWindow* windowPtr = Controller::GetInstance().mGlWindow;
//      float xScale(windowPtr->w() / 2.0f), yScale(-windowPtr->h()/2.0f);
//      Mat4 worldToScreen = ViewScene::GetInstance().mTransform.GetFullTransform();
//      Mat4 screenToWorld = worldToScreen.inverse();
//      Mat4 test = worldToScreen * screenToWorld;
//      
//      pos.x = pos.x / xScale - 1;
//      pos.y = pos.y / yScale + 1; 
//      Vec3 cursor(pos.x, pos.y, 1.0f);
//      orig = Controller::GetInstance().GetCamera()->mViewPoint.Origin();
//      cursor = screenToWorld * cursor;
//      //camera = screenToWorld * camera;
//      dir = cursor - orig;
//   }
//   void Project(Vec3 pos, Vec3& screen)
//   {
//      GlWindow* windowPtr = Controller::GetInstance().mGlWindow;
//      float xScale(windowPtr->w() / 2.0f), yScale(-windowPtr->h()/2.0f);
//      Mat4 worldToScreen = ViewScene::GetInstance().mTransform.GetFullTransform();
//      Vec3 cursor(pos.x, pos.y, 1.0f);
//      //orig = Controller::GetInstance().GetCamera()->mViewPoint.Origin();
//      screen = worldToScreen * pos;
//      pos.x = (pos.x + 1) * xScale;
//      pos.y = (pos.y - 1) * yScale; 
//      //dir = cursor - orig;
//   }
//}
int ControlPoint::handle(int event)
{
   int r = 0;
   if (mFreeTranslateMode)
   {
      if (event == FL_PUSH && Fl::event_button1())
      {
         mSavedPoint = mPoint;
         Vec2 downPoint(Fl::event_x(), Fl::event_y());
         Vec3 orig, dir;
         UiUtil::WindowToWorld(downPoint, orig, dir);
      }
      else if (event == FL_DRAG && Fl::event_button1())
      {
         Vec2 mPos(Fl::event_x(), Fl::event_y());
         Vec3 orig, dir;
         UiUtil::WindowToWorld(mPos, orig, dir);
         float len = (mSavedPoint - orig).length();
         Vec3 np = (dir.normal() * len) + orig;
         ViewScene::GetInstance().GetSelectedMesh()->MoveSelectedVerts(np - mPoint);
         mPoint = np;
         r = 1;
      }
      else if (event == FL_RELEASE)
      {
         ViewScene::GetInstance().GetSelectedMesh()->Rebuild();
         ViewScene::SelectedMeshChanged();
         SetModeP(mCurrentMode);
         r = 1;
      }
   }
   if (mRotationMode != 0)
   {
      r = HandleRotation(event);
   }
   if (mScaleMode != 0)
   {
      r = HandleScale(event);
   }
   if (mTranslateMode)
   {
      if (event == FL_PUSH && Fl::event_button1())
      {
         GlWindow* windowPtr = Controller::GetInstance().mGlWindow;
         Vec2 downPoint(Fl::event_x(), Fl::event_y());
         Vec3 orig, dir;
         UiUtil::WindowToWorld(downPoint, orig, dir);
         {
            Vec3 toPoint = mPoint - mViewPoint->Origin();
            float distance = toPoint.length();

            for (int i = 0; i < 3; ++i)
            {
               Vec3 moveVec;
               moveVec[i] = 1;
               if (ArrowIntersect(orig, dir, moveVec))
               {
                  mAxisDown = i;
                  mSavedPoint = mPoint;
                  r = 1;
                  bool hit=true;
               }
            }
            
         }
      }
      else if (event == FL_DRAG && Fl::event_button1())
      {
         Vec2 mPos(Fl::event_x(), Fl::event_y());
         Vec3 orig, dir;
         UiUtil::WindowToWorld(mPos, orig, dir);
         if (mAxisDown > -1)
         {
            Vec3 hitPoint;
            Plane p;
            p.point = mSavedPoint;
            p.normal = (mSavedPoint - mViewPoint->Origin()).normal();
            if (Intersector::RayPlane(orig, dir, p, hitPoint))
            {
               Vec3 moveDir;
               moveDir[mAxisDown] = 1;
               float moveDistance = dot((hitPoint - mSavedPoint), moveDir);
               Vec3 newPoint = mSavedPoint + moveDir * moveDistance;
               ViewScene::GetInstance().GetSelectedMesh()->MoveSelectedVerts(newPoint - mPoint);
               //mController->MoveSelectedVerts(newPoint - mPoint);
               mPoint = newPoint;
            }
            r = 1;
         }
      }
      else if (event == FL_RELEASE)
      {
         if (mAxisDown != -1)
         {
            ViewScene::GetInstance().GetSelectedMesh()->Rebuild();
            ViewScene::SelectedMeshChanged();
            //mController->RefreshMesh();
            SetModeP(mCurrentMode);
         }
         mAxisDown = -1;
         r = 1;
      }
   }
   
   if (!r && event == FL_KEYDOWN)
   {
/*      if (Fl::event_key() == 't')
      {
         mTranslateMode = !mTranslateMode;
         r = 1;
      }
      else */
      if (Fl::event_key() == 't')
      {
         SetMode(cTRANSLATE);
         r = 1;
      }
      if (Fl::event_key() == 'r')
      {
         SetMode(cROTATE);
         r = 1;
      }
      if (Fl::event_key() == 's')
      {
         SetMode(cSCALE);
         r = 1;
      }
   }

   return r;
}
void ControlPoint::HandleSelectionChange()
{
   mIsVisible = false;
   MeshView* meshView = ViewScene::GetInstance().GetSelectedMesh();
   if (!meshView) return;
   std::vector<int> verts;
   meshView->GetSelectedVerts(verts);
   size_t selCount = verts.size();
   if (selCount < 1) return;
   mPoint = Vec3();
   PolyMesh& m = meshView->mMesh;
   size_t vertCount = m.verts.size();
   for (size_t i = 0; i < selCount; ++i)
   {
      int v = verts[i];
      if (v < vertCount)
      {
         mPoint += m.verts[v];
      }
   }
   //if (meshView->mSelectedInterface)
   mPoint /= selCount;
   mIsVisible = true;
}

void ControlPoint::DrawWindow()
{
   if (mRotationMode>1)
   {
      gl::NoDepthTest state1;
      gl::NoLighting state2;
      glColor3f(0,0,0);
      //glLineWidth(1);
      //GlWindow* windowPtr = Controller::GetInstance().mGlWindow;
      static float lastAngle = 0.0f;
      //float xScale(windowPtr->w() / 2.0f), yScale(-windowPtr->h()/2.0f);
      Vec3 rC;
      UiUtil::WorldToWindow(mSavedPoint, rC); 
      //Project(mSavedPoint, rC);
      //rC[0] = (rC[0] + 1) * xScale;
      //rC[1] = (rC[1] - 1) * yScale;
      //Vec2 downPoint(Fl::event_x(), Fl::event_y());
      glBegin(GL_LINES);
      glVertex3f(rC[0], rC[1], 0);
      glVertex3f(mScreenPoint[0], mScreenPoint[1], 0);
      glEnd();
   }
}

void ControlPoint::Draw()
{
   if (! mIsVisible) return;
   gl::NoDepthTest state1;
   gl::NoLighting state2;
   glColor3f(1,0,0);
   glPointSize(5);
   glBegin(GL_POINTS);
   gl::Vertex(mPoint);
   glEnd();

   Vec3 toPoint = mPoint - mViewPoint->Origin();
   float distance = toPoint.length();
   toPoint /= distance;
   Vec3 vx, vy;
   ortho(toPoint, vx, vy);

   vx *= distance / 30;
   vy *= distance / 30;

   glPushMatrix();
   gl::Translate(mPoint);
   glBegin(GL_LINE_STRIP);
   for (int i = 0; i < 35; ++i)
   {
      float a = i / 34.0f * Math::cTWO_PI;
      gl::Vertex(sin(a) * vx + cos(a) * vy);
   }
   glEnd();   
   glPopMatrix();
   if (mTranslateMode)
   {
      DrawArrow(Vec3(1,0,0));
      glColor3f(0,1,0);
      DrawArrow(Vec3(0,1,0));
      glColor3f(0,0,1);
      DrawArrow(Vec3(0,0,1));
   }
}

void ControlPoint::DrawArrow(Vec3 aDirection)
{
   Vec3List verts;
   GetArrowVerts(aDirection, verts);
   gl::NoCull state;
   glBegin(GL_TRIANGLES);
   gl::Vertex(verts);
   /*gl::Vertex(-.5 * aX);
   gl::Vertex(-.5 * aX + 2 * aY);
   gl::Vertex(.5 * aX + 2 * aY);

   gl::Vertex(.5 * aX + 2 * aY);
   gl::Vertex(-.5 * aX);
   gl::Vertex(.5 * aX);

   gl::Vertex(-.7 * aX + 2 * aY);
   gl::Vertex(.7 * aX + 2 * aY);
   gl::Vertex(2.7 * aY);*/
   glEnd();
}
void ControlPoint::GetArrowVerts(Vec3               aDirection,
                                 std::vector<Vec3>& aVerts)
{
   Vec3 toPoint = mPoint - mViewPoint->Origin();
   float distance = toPoint.length();
   toPoint /= distance;
   Vec3 xx = cross(toPoint, aDirection).normal();

   float arrowScale = distance / 50;
   GetArrowVerts(xx * arrowScale, aDirection * arrowScale, aVerts);
}
void ControlPoint::GetArrowVerts(Vec3               aX,
                                 Vec3               aY,
                                 std::vector<Vec3>& aVerts)
{
   aVerts.push_back(-.5 * aX);
   aVerts.push_back(-.5 * aX + 2 * aY);
   aVerts.push_back(.5 * aX + 2 * aY);

   aVerts.push_back(.5 * aX + 2 * aY);
   aVerts.push_back(-.5 * aX);
   aVerts.push_back(.5 * aX);

   aVerts.push_back(-.7 * aX + 2 * aY);
   aVerts.push_back(.7 * aX + 2 * aY);
   aVerts.push_back(2.7 * aY);
   for (size_t i = 0; i < aVerts.size(); ++i)
   {
      aVerts[i] += mPoint;
   }
}

bool ControlPoint::ArrowIntersect(Vec3 aRayOrigin,
                                  Vec3 aRay,
                                  Vec3 aDirection)
{
   using namespace MeshAlgorithm;

   Vec3List verts;
   GetArrowVerts(aDirection, verts);
   for (size_t i = 0; i < verts.size()-2; i+=3)
   {
      float t,u,v;
      if (math::is_finite(verts[i]) && math::is_finite(verts[i+1]) && math::is_finite(verts[i+2]))
      {
         if (RayTriangleIntersect(aRayOrigin, aRay, verts[i], verts[i+1], verts[i+2],t,u,v)) return true;
         if (RayTriangleIntersect(aRayOrigin, aRay, verts[i+2], verts[i+1], verts[i],t,u,v)) return true;
      }
   }
   return false;
}
int ControlPoint::HandleRotation(int event)
{
   int r(0);
   GlWindow* windowPtr = Controller::GetInstance().mGlWindow;
   static float lastAngle = 0.0f;
   float xScale(windowPtr->w() / 2.0f), yScale(-windowPtr->h()/2.0f);
   mScreenPoint.set(Fl::event_x(), Fl::event_y());
   if (event == FL_PUSH && Fl::event_button1())
   {
      mSavedPoint = mPoint;
      Vec3 rC;
      UiUtil::WorldToWindow(mSavedPoint, rC);
      Vec2 downPoint(Fl::event_x(), Fl::event_y());
      lastAngle = atan2(downPoint[1] - rC[1], downPoint[0] - rC[0]);
      Vec3 orig, dir;
      UiUtil::WindowToWorld(downPoint, orig, dir);
      ++mRotationMode;
   }
   else if (event == FL_DRAG && Fl::event_button1())
   {
      MeshView* view = ViewScene::GetInstance().GetSelectedMesh();
      Vec3 rC;
      UiUtil::WorldToWindow(mSavedPoint, rC);
      //Project(mSavedPoint, rC); 
      std::vector<int>& vertList = ViewScene::GetInstance().GetSelectedMesh()->GetSelectedVertices();
      Vec2 downPoint(Fl::event_x(), Fl::event_y());
      float angle = atan2(downPoint[1] - rC[1], downPoint[0] - rC[0]);
      float angleChange = angle - lastAngle;
      lastAngle = angle;
      VertexList& V = *view->mMesh.verts;
      Mat4 rot = math::translate4(-mSavedPoint)
         * math::rotation(angleChange, mViewPoint->Forward())
         * math::translate4(mSavedPoint);
      for (size_t i = 0; i < vertList.size(); ++i)
      {
         Vec3& v = V[vertList[i]];
         v = v * rot;
      }
      view->RebuildBase();
      r = 1;
   }
   else if (event == FL_RELEASE)
   {
      ViewScene::GetInstance().GetSelectedMesh()->Rebuild();
      ViewScene::SelectedMeshChanged();
      SetModeP(mCurrentMode);
      r = 1;
   }
   return r;
}

int ControlPoint::HandleScale(int event)
{
   int r(0);
   GlWindow* windowPtr = Controller::GetInstance().mGlWindow;
   static float startDistance;
   static float lastDistance;
   float xScale(windowPtr->w() / 2.0f), yScale(-windowPtr->h()/2.0f);
   mScreenPoint.set(Fl::event_x(), Fl::event_y());
   if (event == FL_PUSH && Fl::event_button1())
   {
      mSavedPoint = mPoint;
      Vec3 rC;
      UiUtil::WorldToWindow(mSavedPoint, rC);
      Vec2 downPoint(Fl::event_x(), Fl::event_y());
      startDistance = (Vec2(rC) - downPoint).length();
      lastDistance = startDistance;
      Vec3 orig, dir;
      UiUtil::WindowToWorld(downPoint, orig, dir);
      ++mScaleMode;
   }
   else if (event == FL_DRAG && Fl::event_button1())
   {
      MeshView* view = ViewScene::GetInstance().GetSelectedMesh();
      Vec3 rC;
      UiUtil::WorldToWindow(mSavedPoint, rC);
      //Project(mSavedPoint, rC); 
      std::vector<int>& vertList = ViewScene::GetInstance().GetSelectedMesh()->GetSelectedVertices();
      Vec2 downPoint(Fl::event_x(), Fl::event_y());
      float dist = (Vec2(rC) - downPoint).length();
      float newScale = dist / startDistance;
      float lastScale = lastDistance / startDistance;
      float scaleChange = newScale / lastScale;
      Mat4 scale = math::translate4(-mSavedPoint)
         * math::scale4(scaleChange, scaleChange, scaleChange)
         * math::translate4(mSavedPoint);
      lastDistance = dist;
      VertexList& V = *view->mMesh.verts;
      for (size_t i = 0; i < vertList.size(); ++i)
      {
         Vec3& v = V[vertList[i]];
         v = v * scale;
      }
      view->RebuildBase();
      r = 1;
   }
   else if (event == FL_RELEASE)
   {
      ViewScene::GetInstance().GetSelectedMesh()->Rebuild();
      ViewScene::SelectedMeshChanged();
      SetModeP(mCurrentMode);
      r = 1;
   }
   return r;
}

void ControlPoint::SetModeP(Mode aMode)
{
   mTranslateMode = mFreeTranslateMode = false;
   mScaleMode = mRotationMode = 0;
   switch (aMode)
   {
   case cROTATE: mRotationMode = 1; 
      Controller::GetInstance().SetStatus("Rotate [ mouse ]");
      break;
   case cSCALE: mScaleMode = 1; 
      Controller::GetInstance().SetStatus("Scale [ mouse ]");
      break;
   case cTRANSLATE: mFreeTranslateMode = true; 
      Controller::GetInstance().SetStatus("Translate [ mouse ]");
      break;
   default:
      Controller::GetInstance().SetStatus("");
      break;
   }
   mCurrentMode = aMode;
}
